/*
 * Robot.cpp
 *
 *  Created on: Feb 19, 2011
 *      Author: Anton
 */

#include <motion/Robot.h>

Robot::Robot() {
	//cout<<"Robot"<<endl;
	legs = new Leg[LEGS];
	x = 0;
	y = 0;
	z = 0;
	speed = 0;
	speedwait = 0;
	oldspeed = 0;
	width = 0;
	length = 0;
	height = 0;
	vl = 0;
	fl = 0;
	hl = 0;
	bl = 0;
	Robot::setOrientation(5);
	logbool=0;
}

Robot::~Robot() {
	//cout << "\tDestroy Legs" << endl;
	legs = NULL;
	delete[] legs;
	//cout << "Legs Destroyed" << endl;
}

double Robot::getHeight() const {
	return height;
}

double Robot::getLength() const {
	return length;
}

double Robot::getOldspeed() const {
	return oldspeed;
}

double Robot::getSpeed() const {
	return speed;
}

double Robot::getMass() const {
	return mass;
}
double Robot::getSpeedWait() const{
return speedwait;
}

void Robot::setSpeedWait(double sp){
this->speedwait=sp;
}

double Robot::getWidth() const {
	return width;
}

double Robot::getX() const {
	return x;
}

double Robot::getY() const {
	return y;
}

double Robot::getZ() const {
	return z;
}

void Robot::setHeight(double height) {
	this->height = height;
}

void Robot::setLength(double length) {
	this->length = length;
}

void Robot::setOldspeed(double oldspeed) {
	this->oldspeed = oldspeed;
}

void Robot::setSpeed(double speed) {
	this->speed = speed;
}

void Robot::setWidth(double width) {
	this->width = width;
}

void Robot::setX(double x) {
	this->x = x;
}

void Robot::setY(double y) {
	this->y = y;
}

void Robot::setZ(double z) {
	this->z = z;
}

void Robot::printInfo() {
	for (int i = 0; i < LEGS; i++) {
		cout << "DesiredX: " << Robot::legs[i].getDesiredx() << endl;
		cout << "DesiredY: " << Robot::legs[i].getDesiredy() << endl;
		cout << "DesiredZ: " << Robot::legs[i].getDesiredz() << endl;

		for (int j = 0; j < LINKS; j++) {
			cout << "Leg " << i << " Link " << j << endl;
			Robot::legs[i].links[j].printInfo();
		}
	}

	string heading;

	switch (Robot::orientation) {
	case 1:
		heading = "STOP";
		break;
	case 3:
		heading = "RIGHT";
		break;
	case 4:
		heading = "LEFT";
		break;
	case 5:
		heading = "FORWARD";
		break;
	case 6:
		heading = "BACKWARD";
		break;
	}
	cout << "Robot's heading: " << heading << endl;
	cout << vl << " " << fl << " " << bl << " " << hl << endl;

}

double Robot::getHl() const {
	return hl;
}

void Robot::setHl(double hl) {
	this->hl = hl;
}

double Robot::getBl() const {
	return bl;
}

double Robot::getFl() const {
	return fl;
}

double Robot::getVl() const {
	return vl;
}

void Robot::setBl(double bl) {
	this->bl = bl;
}

void Robot::setFl(double fl) {
	this->fl = fl;
}

void Robot::setVl(double vl) {
	this->vl = vl;
}

int Robot::getOrientation() const {
	return orientation;
}

void Robot::setOrientation(int orientation) {
	this->orientation = orientation;
	switch (orientation) {
	//Translates forward
	case 5:
		vl = 1.0;
		hl = 1.0;
		break;
		//Translates backwards
	case 6:
		vl = -1.0;
		hl = 1.0;
		break;
		//Translates left
	case 4:
		fl = -1.0;
		hl = 1.0;
		break;
		//Translates right
	case 3:
		fl = 1.0;
		bl = 1.0;
		break;
		//STOP
	case 1:
		vl = 0.0;
		fl = 0.0;
		bl = 0.0;
		hl = 0.0;
		break;
	}
}

Matrix Robot::forwardK() {
	Matrix C = Matrix(4, 3, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
			0.0, 0.0);
	for (int i = 0; i < LEGS; i++) {
		Matrix A = Robot::legs[i].forwardK();
		C.setvalue(i, 0, A.getvalue(0, 3));
		C.setvalue(i, 1, A.getvalue(1, 3));
		C.setvalue(i, 2, A.getvalue(2, 3));
	}
	//cout << "ROBOT: " << C << endl;
	return C;
}

void Robot::inverseK() {
	for (int i = 0; i < LEGS; i++) {
		Matrix A = Robot::legs[i].inverseK();

		//POSITIONS

		//LIMIT SWITCHES
		//FL2 -30 / 60
		//FL3 -80 / 80
		//BL2 -60 / 50
		//BL3 25 / 120
		
		if(i==0){
			if(A.getvalue(0,1)<-30){
				cout << "0,1,-30 ... error: " << A.getvalue(0,1) << endl;
				A.setvalue(0,1,-30);
			}
			if(A.getvalue(0,1)>60){
				cout << "0,1,60 ... error: " << A.getvalue(0,1) << endl;
				A.setvalue(0,1,60);
			}
			if(A.getvalue(0,1)<0){
			if(A.getvalue(0,2)<-80){
				cout << "0,2,-80 ... error: " << A.getvalue(0,2) <<  "   0,1: " << A.getvalue(0,1) << endl;
				A.setvalue(0,2,-80);
			}
			if(A.getvalue(0,2)>80){
				cout << "0,2,80 ... error: " << A.getvalue(0,2) <<  "   0,1: " << A.getvalue(0,1) << endl;
				A.setvalue(0,2,80);
			}}
			if(A.getvalue(0,1)>0){
			if(A.getvalue(0,2)<-95){
				cout << "0,2,-95 ... error: " << A.getvalue(0,2) <<  "   0,1: " << A.getvalue(0,1) << endl;
				A.setvalue(0,2,-95);
			}
			if(A.getvalue(0,2)>95){
				cout << "0,2,95 ... error: " << A.getvalue(0,2) <<  "   0,1: " << A.getvalue(0,1) << endl;
				A.setvalue(0,2,95);
			}}
		}
		
		if(i==1){
			if(A.getvalue(0,1)<-30){
				cout << "1,1,-30 ... error: " << A.getvalue(0,1) << endl;
				A.setvalue(0,1,-30);
			}
			if(A.getvalue(0,1)>60){
				cout << "1,1,60 ... error: " << A.getvalue(0,1) << endl;
				A.setvalue(0,1,60);
			}
			if(A.getvalue(0,1)<0){
			if(A.getvalue(0,2)<-80){
				cout << "1,2,-80 ... error: " << A.getvalue(0,2) <<  "   1,1: " << A.getvalue(0,1) << endl;
				A.setvalue(0,2,-80);
			}
			if(A.getvalue(0,2)>80){
				cout << "1,2,80 ... error: " << A.getvalue(0,2) <<  "   1,1: " << A.getvalue(0,1) << endl;
				A.setvalue(0,2,80);
			}}
			if(A.getvalue(0,1)>0){
			if(A.getvalue(0,2)<-95){
				cout << "1,2,-95 ... error: " << A.getvalue(0,2) <<  "   1,1: " << A.getvalue(0,1) << endl;
				A.setvalue(0,2,-95);
			}
			if(A.getvalue(0,2)>95){
				cout << "1,2,95 ... error: " << A.getvalue(0,2) <<  "   1,1: " << A.getvalue(0,1) << endl;
				A.setvalue(0,2,95);
			}}
		}
		
		if(i==2){
			if(A.getvalue(0,1)<-60){
				cout << "2,1,-60 ... error: " << A.getvalue(0,1) << endl;
				A.setvalue(0,1,-60);
			}
			if(A.getvalue(0,1)>50){
				cout << "2,1,50 ... error: " << A.getvalue(0,1) << endl;
				A.setvalue(0,1,50);
			}
			if(A.getvalue(0,2)<25){
				cout << "2,2,25 ... error: " << A.getvalue(0,2) <<   "   2,1: " << A.getvalue(0,1) <<endl;
				A.setvalue(0,2,25);
			}
			if(A.getvalue(0,2)>120){
				cout << "2,2,120 ... error: " << A.getvalue(0,2) <<   "   2,1: " << A.getvalue(0,1) <<endl;
				A.setvalue(2,2,120);
			}
		}

		if(i==3){
			if(A.getvalue(0,1)<-60){
				cout << "3,1,-60 ... error: " << A.getvalue(0,1) << endl;
				A.setvalue(0,1,-60);
			}
			if(A.getvalue(0,1)>50){
				cout << "3,1,50 ... error: " << A.getvalue(0,1) << endl;
				A.setvalue(0,1,50);
			}
			if(A.getvalue(0,2)<25){
				cout << "3,2,25 ... error: " << A.getvalue(0,2) <<   "   3,1: " << A.getvalue(0,1) <<endl;
				A.setvalue(0,2,25);
			}
			if(A.getvalue(0,2)>120){
				cout << "3,2,120 ... error: " << A.getvalue(0,2) <<  "   3,1: " << A.getvalue(0,1) << endl;
				A.setvalue(0,2,120);
			}
		}
		
				
		//Set old Q values
		Robot::legs[i].links[0].setOld(Robot::legs[i].links[0].getQ());
		Robot::legs[i].links[1].setOld(Robot::legs[i].links[1].getQ());
		Robot::legs[i].links[2].setOld(Robot::legs[i].links[2].getQ());
		//Set new Q values
		Robot::legs[i].links[0].setQ(A.getvalue(0, 0));
		Robot::legs[i].links[1].setQ(A.getvalue(0, 1));
		Robot::legs[i].links[2].setQ(A.getvalue(0, 2));

		//VELOCITIES

		//Set old dQ values
		Robot::legs[i].links[0].setDq_old(Robot::legs[i].links[0].getDq());
		Robot::legs[i].links[1].setDq_old(Robot::legs[i].links[1].getDq());
		Robot::legs[i].links[2].setDq_old(Robot::legs[i].links[2].getDq());
		//Set new dQ values
		Robot::legs[i].links[0].setDq(Robot::legs[i].links[0].getQ()
				- Robot::legs[i].links[0].getOld());
		Robot::legs[i].links[1].setDq(Robot::legs[i].links[1].getQ()
				- Robot::legs[i].links[1].getOld());
		Robot::legs[i].links[2].setDq(Robot::legs[i].links[2].getQ()
				- Robot::legs[i].links[2].getOld());

		//ACCELERATIONS

		//Set old ddQ values
		Robot::legs[i].links[0].setDdq_old(Robot::legs[i].links[0].getDdq());
		Robot::legs[i].links[1].setDdq_old(Robot::legs[i].links[1].getDdq());
		Robot::legs[i].links[2].setDdq_old(Robot::legs[i].links[2].getDdq());
		//Set new ddQ values
		Robot::legs[i].links[0].setDdq(Robot::legs[i].links[0].getDq()
				- Robot::legs[i].links[0].getDq_old());
		Robot::legs[i].links[1].setDdq(Robot::legs[i].links[1].getDq()
				- Robot::legs[i].links[1].getDq_old());
		Robot::legs[i].links[2].setDdq(Robot::legs[i].links[2].getDq()
				- Robot::legs[i].links[2].getDq_old());

	}
}

void Robot::printIK() {
	for (int i = 0; i < LEGS; i++) {

		cout << "Leg " << i << " Q: " << Robot::legs[i].links[0].getQ() << " "
				<< Robot::legs[i].links[1].getQ() << " "
				<< Robot::legs[i].links[2].getQ() << endl;
		cout << "Leg " << i << " dQ: " << Robot::legs[i].links[0].getDq()
				<< " " << Robot::legs[i].links[1].getDq() << " "
				<< Robot::legs[i].links[2].getDq() << endl;
		cout << "Leg " << i << " ddQ: " << Robot::legs[i].links[0].getDdq()
				<< " " << Robot::legs[i].links[1].getDdq() << " "
				<< Robot::legs[i].links[2].getDdq() << endl;

	}
}

void Robot::logIK() {

	ofstream mylog;

	if (logbool == 0) {
		mylog.open("logIK.txt", ios::out | ios::trunc);
		logbool = 1;
	} else {
		mylog.open("logIK.txt", ios::out | ios::app);
	}
	for(int i=0;i<LEGS;i++){
	mylog << Robot::legs[i].links[0].getQ() << endl
			<< Robot::legs[i].links[1].getQ() << endl
			<< Robot::legs[i].links[2].getQ() << endl
			<< Robot::legs[i].links[0].getDq() << endl
			<< Robot::legs[i].links[1].getDq() << endl
			<< Robot::legs[i].links[2].getDq() << endl
			<< Robot::legs[i].links[0].getDdq() << endl
			<< Robot::legs[i].links[1].getDdq() << endl
			<< Robot::legs[i].links[2].getDdq() << endl;
	}

	mylog.close();

}

